from machine import Pin, PWM


class TB662Driver:
    def __init__(self, pin_pwm, pin_an1, pin_an2):
        self.pwm = PWM(Pin(pin_pwm), freq=1000, duty=0)  # 初始占空比0%
        # 控制引脚
        self.ain1 = Pin(pin_an1, Pin.OUT)
        self.ain2 = Pin(pin_an2, Pin.OUT)

    def set_motor_speed(self, speed: int):
        """
        set speed
        :param speed: PWM占空比  0~1023
        :return:
        """
        if speed > 0:
            self.ain1.on()
            self.ain2.off()
            self.pwm.duty(speed)
        elif speed < 0:
            self.ain1.off()
            self.ain2.on()
            self.pwm.duty(-speed)
        else:
            self.ain1.on()
            self.ain2.off()
            self.pwm.duty(speed)


class MotorDriver:
    def __init__(self):
        self.right_motor = TB662Driver(13, 12, 14)
        self.left_motor = TB662Driver(27, 25, 26)

    def set_left_motor_speed(self, speed):
        self.left_motor.set_motor_speed(speed)

    def set_right_motor_speed(self, speed):
        self.right_motor.set_motor_speed(speed)


if __name__ == '__main__':
    motor = MotorDriver()
    motor.set_right_motor_speed(1000)
    motor.set_left_motor_speed(1000)
